#include <stdlib.h>

#include "dsp.h"

#include <util/common_macros.h>
#include <util/msg.h>
#include <util/mem.h>

// =============================================================================
static int * __create_int_sequence(int size, int start, int increment) {
	int i, inc;
	int * shift;

	shift = mem_allocn(size, int);
	for (i = 0, inc = start; i < size; i++, inc += increment) shift[i] = inc;

	return shift;
}

// =============================================================================
void convolve(real * M, int size, real * W, int wsize, real * Out) {

	int i, wi;
	int wc = wsize >> 1;
	int wi_max = wsize - 1;
	int size_minus_wc = size - wc;

	real * ptr = &M[wc];

	for (i = wc; i < size_minus_wc; i++) {

		*Out = 0;

		for (wi = -wc; wi <= wc; wi++) {
			*Out = *(ptr + wc) * *(W + (wi_max - wi));
		}
		ptr++;
		Out++;
	}
}
void convolve_zero_padding(real * M, int size, real * W, int wsize, real * Out) {

	int i, d, wi, di;
	int wc = wsize >> 1;
	int wi_max = wsize - 1;
	int size_minus_wc = size - wc;

	for (i = wc; i < size - wc; i++) {

		Out[i] = 0;

		for (wi = 0; wi < size_minus_wc; wi++) {
			d = i + (wi - wc);
			di = wi_max - wi;
			Out[i] += M[d] * W[ di ];
		}
	}
}
void convolve_reflect_border(real * M, int size, real * W, int wsize, real * Out) {

	int i, d, wi, di;
	int wc = wsize >> 1;
	int wi_max = wsize - 1;

	for (i = 0; i < size; i++) {

		Out[i] = 0;

		for (wi = 0; wi < wsize; wi++) {
			d = i + (wi - wc);

			// Border reflection
			if (d < 0) d = size + d % size;
			else if (d >= size) d = d % size;

			di = wi_max - wi;
			Out[i] += M[d] * W[ di ];
		}

	}
}
void convolve_region(real * M, int size, real * W, int wsize, real * Out, int i0, int i1) {

	int i, d, wi, di;
	int wc = wsize >> 1;
	int wi_max = wsize - 1;

	for (i = i0; i <= i1; i++) {

		Out[i] = 0;

		for (wi = 0; wi < wsize; wi++) {
			d = i + (wi - wc);

			// Border reflection
			if (d < 0) d = size + d % size;
			else if (d >= size) d = d % size;

			di = wi_max - wi;
			Out[i] += M[d] * W[ di ];
		}

	}
}
void convolve_border_reflect_fast(real * M, int size, real * W, int wsize, real * Out) {

	int wc = wsize >> 1;

	convolve_region(M, size, W, wsize, Out, 0, wc - 1);
	convolve_zero_padding(M, size, W, wsize, Out);
	convolve_region(M, size, W, wsize, Out, wsize - wc, wsize - 1);

}

// =============================================================================
/**
 * 
 * C(x,y) = \sum_{u=-\frac{M}{2}}^{M} \sum_{v=-\frac{N}{2}}^{N}  \left( w(u + M/2,v + N/2) \cdot I(x-u,y-v)  \right)
 * 
 * @param M
 * @param rows
 * @param columns
 * @param W
 * @param wrows
 * @param wcolumns
 * @param Out
 */
void convolve2D_avoid_border(real * M, int rows, int columns, real * W, int wrows, int wcolumns, real * Out) {

	int r, c, _r, i;
	int wr, wc, _wr, wi;
	int rf, cf, d;

	int wrc = (wrows - 1) / 2;
	int wcc = (wcolumns - 1) / 2;

	if (wrc == 0) wrc = 1;
	if (wcc == 0) wcc = 1;

	int wi_max = (wrows * wcolumns) - 1;

	real sum;
	int * stride_shift = __create_int_sequence(rows, 0, columns);

	for (r = wrc, _r = r * columns; r < rows - wrc; r++, _r += columns) {
		for (c = wcc; c < columns - wcc; c++) {
			i = _r + c;

			sum = 0.0;

			for (wr = 0, _wr = wr * wcolumns; wr < wrows; wr++, _wr += wcolumns) {

				// Avoid invalid row index
				rf = r + (wr - wrc);

				for (wc = 0; wc < wcolumns; wc++) {
					wi = _wr + wc;

					// Avoid invalid column index
					cf = c + (wc - wcc);

					// Gets the respective stride to row rf
					d = stride_shift[rf] + cf;

					// reflect the mask index. Because it is a convolve!
					wi = wi_max - wi;

					sum += M[d] * W[wi];
				}
			}
			Out[i] = sum;
		}
	}
}
void convolve2D_avoid_border_with_mask(real * M, int rows, int columns, real * W, int wrows, int wcolumns, real * Mask, real * Out) {

	int r, c, _r, i;
	int wr, wc, _wr, wi;
	int rf, cf, d;

	int wrc = (wrows - 1) / 2;
	int wcc = (wcolumns - 1) / 2;

	if (wrc == 0) wrc = 1;
	if (wcc == 0) wcc = 1;

	int wi_max = (wrows * wcolumns) - 1;

	real sum;
	int * stride_shift = __create_int_sequence(rows, 0, columns);

	for (r = wrc, _r = r * columns; r < rows - wrc; r++, _r += columns) {
		for (c = wcc; c < columns - wcc; c++) {
			i = _r + c;

			if (Mask[i] == 0) continue;

			sum = 0.0;

			for (wr = 0, _wr = wr * wcolumns; wr < wrows; wr++, _wr += wcolumns) {

				// Avoid invalid row index
				rf = r + (wr - wrc);

				for (wc = 0; wc < wcolumns; wc++) {
					wi = _wr + wc;

					// Avoid invalid column index
					cf = c + (wc - wcc);

					// Gets the respective stride to row rf
					d = stride_shift[rf] + cf;

					// reflect the mask index. Because it is a convolve!
					wi = wi_max - wi;

					sum += M[d] * W[wi];
				}
			}
			//Out[i] = sum * Mask[i];
			Out[i] = sum;
		}
	}
}
/**
 * 
 * C(x,y) = \sum_{u=-\frac{M}{2}}^{M} \sum_{v=-\frac{N}{2}}^{N}  \left( w(u + M/2,v + N/2) \cdot I(x-u,y-v)  \right)
 * 
 * @param M
 * @param rows
 * @param columns
 * @param W
 * @param wrows
 * @param wcolumns
 * @param Out
 */
void convolve2D_zero_padding(real * M, int rows, int columns, real * W, int wrows, int wcolumns, real * Out) {

	int r, c, _r, i;
	int wr, wc, _wr, wi;
	int rf, cf, d;

	int wrc = (wrows - 1) / 2;
	int wcc = (wcolumns - 1) / 2;

	if (wrc == 0) wrc = 1;
	if (wcc == 0) wcc = 1;

	int wi_max = (wrows * wcolumns) - 1;

	real sum;
	int * stride_shift = __create_int_sequence(rows, 0, columns);

	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			sum = 0.0;

			for (wr = 0, _wr = wr * wcolumns; wr < wrows; wr++, _wr += wcolumns) {

				// Avoid invalid row index
				rf = r + (wr - wrc);
				if (rf < 0 || rf >= rows) continue;

				for (wc = 0; wc < wcolumns; wc++) {
					wi = _wr + wc;

					// Avoid invalid column index
					cf = c + (wc - wcc);
					if (cf < 0 || cf >= columns) continue;

					// Gets the respective stride to row rf
					d = stride_shift[rf] + cf;

					// reflect the mask index. Because it is a convolve!
					wi = wi_max - wi;

					sum += M[d] * W[wi];
				}
			}
			Out[i] = sum;
		}
	}
}
void convolve2D_zero_padding_with_mask(real * M, int rows, int columns, real * W, int wrows, int wcolumns, real * Mask, real * Out) {

	int r, c, _r, i;
	int wr, wc, _wr, wi;
	int rf, cf, d;

	int wrc = (wrows - 1) / 2;
	int wcc = (wcolumns - 1) / 2;

	if (wrc == 0) wrc = 1;
	if (wcc == 0) wcc = 1;

	int wi_max = (wrows * wcolumns) - 1;

	real sum;
	int * stride_shift = __create_int_sequence(rows, 0, columns);

	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			if (Mask[i] == 0) continue;

			sum = 0.0;

			for (wr = 0, _wr = wr * wcolumns; wr < wrows; wr++, _wr += wcolumns) {

				// Avoid invalid row index
				rf = r + (wr - wrc);
				if (rf < 0 || rf >= rows) continue;

				for (wc = 0; wc < wcolumns; wc++) {
					wi = _wr + wc;

					// Avoid invalid column index
					cf = c + (wc - wcc);
					if (cf < 0 || cf >= columns) continue;

					// Gets the respective stride to row rf
					d = stride_shift[rf] + cf;

					// reflect the mask index. Because it is a convolve!
					wi = wi_max - wi;

					sum += M[d] * W[wi];
				}
			}
			//Out[i] = sum * Mask[i];
			Out[i] = sum;
		}
	}
}
/**
 * 
 * C(x,y) = \sum_{u=-\frac{M}{2}}^{M} \sum_{v=-\frac{N}{2}}^{N}  \left( w(u + M/2,v + N/2) \cdot I(x-u,y-v)  \right)
 * 
 * @param M
 * @param rows
 * @param columns
 * @param W
 * @param wrows
 * @param wcolumns
 * @param Out
 */
void convolve2D_border_reflect(real * M, int rows, int columns, real * W, int wrows, int wcolumns, real * Out) {

	int r, c, _r, i;
	int wr, wc, _wr, wi;
	int rf, cf, d;

	int wrc = (wrows - 1) / 2;
	int wcc = (wcolumns - 1) / 2;

	if (wrc == 0) wrc = 1;
	if (wcc == 0) wcc = 1;

	int _2rows_minus_2 = (rows << 1) - 2;
	int _2columns_minus_2 = (columns << 1) - 2;
	int wi_max = (wrows * wcolumns) - 1;

	real sum;
	int * stride_shift = __create_int_sequence(rows, 0, columns);

	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			sum = 0.0;

			for (wr = 0, _wr = wr * wcolumns; wr < wrows; wr++, _wr += wcolumns) {

				// reflect y boundaries
				rf = r + (wr - wrc);
				if (rf < 0) rf = -rf;
				else if (rf >= rows) rf = _2rows_minus_2 - rf;

				for (wc = 0; wc < wcolumns; wc++) {
					wi = _wr + wc;

					// reflect x boundaries
					cf = c + (wc - wcc);
					if (cf < 0) cf = -cf;
					else if (cf >= columns) cf = _2columns_minus_2 - cf;

					// Gets the respective stride to row rf
					d = stride_shift[rf] + cf;

					// reflect the mask index. Because it is a convolve!
					wi = wi_max - wi;

					sum += M[d] * W[wi];
				}
			}
			Out[i] = sum;
		}
	}
}
void convolve2D_border_reflect_with_mask(real * M, int rows, int columns, real * W, int wrows, int wcolumns, real * Mask, real * Out) {

	int r, c, _r, i;
	int wr, wc, _wr, wi;
	int rf, cf, d;

	int wrc = (wrows - 1) / 2;
	int wcc = (wcolumns - 1) / 2;

	if (wrc == 0) wrc = 1;
	if (wcc == 0) wcc = 1;

	int _2rows_minus_2 = (rows << 1) - 2;
	int _2columns_minus_2 = (columns << 1) - 2;
	int wi_max = (wrows * wcolumns) - 1;

	real sum;
	int * stride_shift = __create_int_sequence(rows, 0, columns);

	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			if (Mask[i] == 0) continue;

			sum = 0.0;

			for (wr = 0, _wr = wr * wcolumns; wr < wrows; wr++, _wr += wcolumns) {

				// reflect y boundaries
				rf = r + (wr - wrc);
				if (rf < 0) rf = -rf;
				else if (rf >= rows) rf = _2rows_minus_2 - rf;

				for (wc = 0; wc < wcolumns; wc++) {
					wi = _wr + wc;

					// reflect x boundaries
					cf = c + (wc - wcc);
					if (cf < 0) cf = -cf;
					else if (cf >= columns) cf = _2columns_minus_2 - cf;

					// Gets the respective stride to row rf
					d = stride_shift[rf] + cf;

					// reflect the mask index. Because it is a convolve!
					wi = wi_max - wi;

					sum += M[d] * W[wi];
				}
			}
			//Out[i] = sum * Mask[i];
			Out[i] = sum;
		}
	}
}
/**
 * \brief This function convolves a matrix W on the matrix M over a defined rectangle region using border reflection.
 * 
 * @param M - input matrix
 * @param rows - number of rows of M
 * @param columns - number of columns of M
 * @param W - window mask used in convolution
 * @param wrows - number of rows of W
 * @param wcolumns - number of columns of W
 * @param Out - output matrix
 * @param r0 - top row index of rectangle
 * @param c0 - left column index of rectangle
 * @param r1 - down row index of rectangle
 * @param c1 - right column index of rectangle
 */
void convolve2D_rectangle_region(real * M, int rows, int columns, real * W, int wrows, int wcolumns, real * Out, int r0, int c0, int r1, int c1) {

	int r, c, _r, i;
	int wr, wc, _wr, wi;
	int rf, cf, d;

	int wrc = (wrows - 1) / 2;
	int wcc = (wcolumns - 1) / 2;

	if (wrc == 0) wrc = 1;
	if (wcc == 0) wcc = 1;

	int _2rows_minus_2 = (rows << 1) - 2;
	int _2columns_minus_2 = (columns << 1) - 2;
	int wi_max = (wrows * wcolumns) - 1;

	//int dr = (r1+wrc) - (r0-wrc) + 1;

	real sum;
	int * stride_shift = __create_int_sequence(rows, 0, columns);

	for (r = r0, _r = r * columns; r <= r1; r++, _r += columns) {
		for (c = c0; c <= c1; c++) {
			i = _r + c;

			sum = 0.0;

			for (wr = 0, _wr = wr * wcolumns; wr < wrows; wr++, _wr += wcolumns) {

				// reflect y boundaries
				rf = r + (wr - wrc);
				if (rf < 0) rf = -rf;
				else if (rf >= rows) rf = _2rows_minus_2 - rf;

				for (wc = 0; wc < wcolumns; wc++) {
					wi = _wr + wc;

					// reflect x boundaries
					cf = c + (wc - wcc);
					if (cf < 0) cf = -cf;
					else if (cf >= columns) cf = _2columns_minus_2 - cf;

					// Gets the respective stride to row rf
					d = stride_shift[rf] + cf;

					// reflect the mask index. Because it is a convolve!
					wi = wi_max - wi;

					sum += M[d] * W[wi];
				}
			}
			Out[i] = sum;
		}
	}
}
void convolve2D_rectangle_region_with_mask(real * M, int rows, int columns, real * W, int wrows, int wcolumns, real * Mask, real * Out, int r0, int c0, int r1, int c1) {

	int r, c, _r, i;
	int wr, wc, _wr, wi;
	int rf, cf, d;

	int wrc = (wrows - 1) / 2;
	int wcc = (wcolumns - 1) / 2;

	if (wrc == 0) wrc = 1;
	if (wcc == 0) wcc = 1;

	int _2rows_minus_2 = (rows << 1) - 2;
	int _2columns_minus_2 = (columns << 1) - 2;
	int wi_max = (wrows * wcolumns) - 1;

	int dr = r1 - r0 + 1;

	real sum;
	int * stride_shift = __create_int_sequence(dr, 0, columns);

	for (r = r0, _r = r * columns; r <= r1; r++, _r += columns) {
		for (c = c0; c <= c1; c++) {
			i = _r + c;

			if (Mask[i] == 0) continue;

			sum = 0.0;

			for (wr = 0, _wr = wr * wcolumns; wr < wrows; wr++, _wr += wcolumns) {

				// reflect y boundaries
				rf = r + (wr - wrc);
				if (rf < 0) rf = -rf;
				else if (rf >= rows) rf = _2rows_minus_2 - rf;

				for (wc = 0; wc < wcolumns; wc++) {
					wi = _wr + wc;

					// reflect x boundaries
					cf = c + (wc - wcc);
					if (cf < 0) cf = -cf;
					else if (cf >= columns) cf = _2columns_minus_2 - cf;

					// Gets the respective stride to row rf
					d = stride_shift[rf - r0] + cf;

					// reflect the mask index. Because it is a convolve!
					wi = wi_max - wi;

					sum += M[d] * W[wi];
				}
			}
			//Out[i] = sum * Mask[i];
			Out[i] = sum;
		}
	}
}
/**
 * \brief This function convolves the matrix W on the matrix M using border reflections only at the border pixels.
 * 
 * This function eliminates the overhead of index check at interior points.
 * 
 * @param M - input matrix
 * @param rows - number of rows of M
 * @param columns - number of columns of M
 * @param W - window mask used in convolution
 * @param wrows - number of rows of W
 * @param wcolumns - number of columns of W
 * @param Out - output matrix
 */
void convolve2D_border_reflect_fast(real * M, int rows, int columns, real * W, int wrows, int wcolumns, real * Out) {

	convolve2D_avoid_border(M, rows, columns, W, wrows, wcolumns, Out);

	int wrc = (wrows - 1) / 2;
	int wcc = (wcolumns - 1) / 2;

	if (wrc == 0) wrc = 1;
	if (wcc == 0) wcc = 1;

	int r0 = wrc;
	int r1 = rows - wrc;
	int c0 = wcc;
	int c1 = columns - wcc;

	convolve2D_rectangle_region(M, rows, columns, W, wrows, wcolumns, Out, 0, 0, r0 - 1, columns - 1);
	convolve2D_rectangle_region(M, rows, columns, W, wrows, wcolumns, Out, r1 + 1, 0, rows - 1, columns - 1);

	convolve2D_rectangle_region(M, rows, columns, W, wrows, wcolumns, Out, r0, 0, r1, c0 - 1);
	convolve2D_rectangle_region(M, rows, columns, W, wrows, wcolumns, Out, r0, c1 + 1, r1, columns - 1);

}
void convolve2D_border_reflect_fast_with_mask(real * M, int rows, int columns, real * W, int wrows, int wcolumns, real * Mask, real * Out) {

	int wrc = (wrows - 1) / 2;
	int wcc = (wcolumns - 1) / 2;

	if (wrc == 0) wrc = 1;
	if (wcc == 0) wcc = 1;

	int r0 = wrc;
	int r1 = rows - wrc;
	int c0 = wcc;
	int c1 = columns - wcc;

	convolve2D_avoid_border_with_mask(M, rows, columns, W, wrows, wcolumns, Mask, Out);

	convolve2D_rectangle_region_with_mask(M, rows, columns, W, wrows, wcolumns, Mask, Out, 0, 0, r0 - 1, columns - 1);
	convolve2D_rectangle_region_with_mask(M, rows, columns, W, wrows, wcolumns, Mask, Out, r1 + 1, 0, rows - 1, columns - 1);

	convolve2D_rectangle_region_with_mask(M, rows, columns, W, wrows, wcolumns, Mask, Out, r0, 0, r1, c0 - 1);
	convolve2D_rectangle_region_with_mask(M, rows, columns, W, wrows, wcolumns, Mask, Out, r0, c1 + 1, r1, columns - 1);

}

// =============================================================================
void convolve2D_hrow(real * M, int rows, int columns, real * W, int wsize, real * Out) {
	int r, c, _r, i;
	int fc;
	int d;

	int wi;
	int wc;
	int max_wi;

	int cl, ch;

	// = Horizontal convolution ================================================
	/*
	 * Regions for horizontal convolution.
	 * In the regions 0 and 2 the algorithm checks the border.
	 *     __________________________________
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |  0  |           1          |  2  |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |_____|______________________|_____|
	 */

	wc = wsize >> 1;
	max_wi = wsize - 1;

	// Horizontal convolution. Region(0)
	cl = 0;
	ch = (wc > columns) ? columns : wc; // Prevents W greater than M
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = cl; c < ch; c++) {
			i = _r + c;

			Out[i] = 0.0;

			for (wi = 0; wi < wsize; wi++) {

				fc = c + (wi - wc);
				if (fc < 0) fc = columns + (fc % columns);
				else if (fc >= columns) fc = (fc % columns);

				d = _r + fc;

				Out[i] += M[d] * W[ max_wi - wi];

			}
		}
	}

	if (wsize <= columns) { // Prevents W greater than M
		// Horizontal convolution. Region(1)	
		for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
			for (c = wc; c < columns - wc; c++) {
				i = _r + c;

				Out[i] = 0.0;

				for (wi = 0; wi < wsize; wi++) {

					fc = c + (wi - wc);
					d = _r + fc;
					Out[i] += M[d] * W[ max_wi - wi];
				}
			}
		}
	}

	if (wc < columns) {

		// Horizontal convolution. Region(2)
		cl = (wsize <= columns) ? (columns - wc) : (wc); // Prevents W greater than M
		ch = columns;
		for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
			for (c = cl; c < ch; c++) {
				i = _r + c;

				Out[i] = 0.0;

				for (wi = 0; wi < wsize; wi++) {

					fc = c + (wi - wc);
					if (fc < 0) fc = columns + (fc % columns);
					else if (fc >= columns) fc = (fc % columns);

					d = _r + fc;

					Out[i] += M[d] * W[ max_wi - wi];

				}
			}
		}
	}
}
void convolve2D_hrow_with_mask(real * M, int rows, int columns, real * W, int wsize, real * Mask, real * Out) {
	int r, c, _r, i;
	int fc;
	int d;

	int wi;
	int wc;
	int max_wi;

	int cl, ch;

	// = Horizontal convolution ================================================
	/*
	 * Regions for horizontal convolution.
	 * In the regions 0 and 2 the algorithm checks the border.
	 *     __________________________________
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |  0  |           1          |  2  |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |     |                      |     |
	 *    |_____|______________________|_____|
	 */

	wc = wsize >> 1;
	max_wi = wsize - 1;

	// Horizontal convolution. Region(0)
	cl = 0;
	ch = (wc > columns) ? columns : wc; // Prevents W greater than M
	for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
		for (c = cl; c < ch; c++) {
			i = _r + c;

			if (Mask[i] == 0) {
				continue;
			}

			Out[i] = 0.0;

			for (wi = 0; wi < wsize; wi++) {

				fc = c + (wi - wc);
				if (fc < 0) fc = columns + (fc % columns);
				else if (fc >= columns) fc = (fc % columns);

				d = _r + fc;

				Out[i] += M[d] * W[ max_wi - wi];

			}
		}
	}

	if (wsize <= columns) { // Prevents W greater than M
		// Horizontal convolution. Region(1)	
		for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
			for (c = wc; c < columns - wc; c++) {
				i = _r + c;

				if (Mask[i] == 0) {
					continue;
				}

				Out[i] = 0.0;

				for (wi = 0; wi < wsize; wi++) {

					fc = c + (wi - wc);
					d = _r + fc;
					Out[i] += M[d] * W[ max_wi - wi];
				}
			}
		}
	}

	if (wc < columns) {

		// Horizontal convolution. Region(2)
		cl = (wsize <= columns) ? (columns - wc) : (wc); // Prevents W greater than M
		ch = columns;
		for (r = 0, _r = r * columns; r < rows; r++, _r += columns) {
			for (c = cl; c < ch; c++) {
				i = _r + c;

				if (Mask[i] == 0) {
					continue;
				}

				Out[i] = 0.0;

				for (wi = 0; wi < wsize; wi++) {

					fc = c + (wi - wc);
					if (fc < 0) fc = columns + (fc % columns);
					else if (fc >= columns) fc = (fc % columns);

					d = _r + fc;

					Out[i] += M[d] * W[ max_wi - wi];

				}
			}
		}
	}
}
void convolve2D_hcolumn(real * M, int rows, int columns, real * W, int wsize, real * Out) {

	int r, c, _r, i;
	int fr;
	int d;

	int wi;
	int wc;
	int max_wi;

	int rl, rh;


	// = Vertical convolution ==================================================
	/*
	 * Regions for vertical convolution.
	 * In the regions 0 and 2 the algorithm checks the border.
	 *     __________________________________
	 *    |                                  |
	 *    |                 0                |
	 *    |__________________________________|
	 *    |                                  |
	 *    |                                  |
	 *    |                                  |
	 *    |                                  |
	 *    |                 1                |
	 *    |                                  |
	 *    |                                  |
	 *    |                                  |
	 *    |__________________________________|
	 *    |                                  |
	 *    |                 2                |
	 *    |__________________________________|
	 */


	wc = wsize >> 1;
	max_wi = wsize - 1;

	// Init vector of
	int * shift = mem_allocn(rows, int);
	int inc;
	for (i = 0, inc = 0; i < rows; i++, inc += columns) shift[i] = inc;

	// Vertical convolution. Region(0)
	rl = 0;
	rh = (wc > rows) ? rows : wc; // Prevents W greater than M
	for (r = rl, _r = r * columns; r < rh; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			Out[i] = 0.0;

			for (wi = 0; wi < wsize; wi++) {

				fr = r + (wi - wc);
				if (fr < 0) fr = rows + (fr % rows);
				else if (fr >= rows) fr = (fr % rows);

				d = shift[fr] + c;

				Out[i] += M[d] * W[ max_wi - wi];

			}
		}
	}

	if (wsize <= columns) { // Prevents W greater than M
		// Vertical convolution. Region(1)
		for (r = wc, _r = r * columns; r < rows - wc; r++, _r += columns) {
			for (c = 0; c < columns; c++) {
				i = _r + c;

				Out[i] = 0.0;

				for (wi = 0; wi < wsize; wi++) {

					fr = r + (wi - wc);
					d = shift[fr] + c;

					Out[i] += M[d] * W[ max_wi - wi];

				}
			}
		}
	}

	if (wc < columns) {

		// Horizontal convolution. Region(2)
		rl = (wsize <= rows) ? (rows - wc) : (wc); // Prevents W greater than M
		rh = rows;
		// Vertical convolution. Region(2)
		for (r = rl, _r = r * columns; r < rh; r++, _r += columns) {
			for (c = 0; c < columns; c++) {
				i = _r + c;

				Out[i] = 0.0;

				for (wi = 0; wi < wsize; wi++) {

					fr = r + (wi - wc);
					if (fr < 0) fr = rows + (fr % rows);
					else if (fr >= rows) fr = (fr % rows);

					d = shift[fr] + c;

					Out[i] += M[d] * W[ max_wi - wi];

				}
			}
		}
	}

	mem_free(shift);
}
void convolve2D_hcolumn_with_mask(real * M, int rows, int columns, real * W, int wsize, real * Mask, real * Out) {

	int r, c, _r, i;
	int fr;
	int d;

	int wi;
	int wc;
	int max_wi;

	int rl, rh;


	// = Vertical convolution ==================================================
	/*
	 * Regions for vertical convolution.
	 * In the regions 0 and 2 the algorithm checks the border.
	 *     __________________________________
	 *    |                                  |
	 *    |                 0                |
	 *    |__________________________________|
	 *    |                                  |
	 *    |                                  |
	 *    |                                  |
	 *    |                                  |
	 *    |                 1                |
	 *    |                                  |
	 *    |                                  |
	 *    |                                  |
	 *    |__________________________________|
	 *    |                                  |
	 *    |                 2                |
	 *    |__________________________________|
	 */


	wc = wsize >> 1;
	max_wi = wsize - 1;

	// Init vector of
	int * shift = mem_allocn(rows, int);
	int inc;
	for (i = 0, inc = 0; i < rows; i++, inc += columns) shift[i] = inc;

	// Vertical convolution. Region(0)
	rl = 0;
	rh = (wc > rows) ? rows : wc; // Prevents W greater than M
	for (r = rl, _r = r * columns; r < rh; r++, _r += columns) {
		for (c = 0; c < columns; c++) {
			i = _r + c;

			if (Mask[i] == 0) {
				continue;
			}

			Out[i] = 0.0;

			for (wi = 0; wi < wsize; wi++) {

				fr = r + (wi - wc);
				if (fr < 0) fr = rows + (fr % rows);
				else if (fr >= rows) fr = (fr % rows);

				d = shift[fr] + c;

				Out[i] += M[d] * W[ max_wi - wi];

			}
		}
	}

	if (wsize <= columns) { // Prevents W greater than M
		// Vertical convolution. Region(1)
		for (r = wc, _r = r * columns; r < rows - wc; r++, _r += columns) {
			for (c = 0; c < columns; c++) {
				i = _r + c;

				if (Mask[i] == 0) {
					continue;
				}

				Out[i] = 0.0;

				for (wi = 0; wi < wsize; wi++) {

					fr = r + (wi - wc);
					d = shift[fr] + c;

					Out[i] += M[d] * W[ max_wi - wi];

				}
			}
		}
	}

	if (wc < columns) {

		// Horizontal convolution. Region(2)
		rl = (wsize <= rows) ? (rows - wc) : (wc); // Prevents W greater than M
		rh = rows;
		// Vertical convolution. Region(2)
		for (r = rl, _r = r * columns; r < rh; r++, _r += columns) {
			for (c = 0; c < columns; c++) {
				i = _r + c;

				if (Mask[i] == 0) {
					continue;
				}

				Out[i] = 0.0;

				for (wi = 0; wi < wsize; wi++) {

					fr = r + (wi - wc);
					if (fr < 0) fr = rows + (fr % rows);
					else if (fr >= rows) fr = (fr % rows);

					d = shift[fr] + c;

					Out[i] += M[d] * W[ max_wi - wi];

				}
			}
		}
	}

	mem_free(shift);
}
void convolve2D_separable(real * M, int rows, int columns, real * W1, int w1size, real * W2, int w2size, real * Out) {

	real * Tmp = mem_allocn(rows*columns, real);

	convolve2D_hcolumn(M, rows, columns, W1, w1size, Tmp);
	convolve2D_hrow(Tmp, rows, columns, W2, w2size, Out);

	mem_free(Tmp);
}
void convolve2D_separable_with_mask(real * M, int rows, int columns, real * W1, int w1size, real * W2, int w2size, real * Mask, real * Out) {

	real * Tmp = mem_allocn(rows*columns, real);

	convolve2D_hcolumn_with_mask(M, rows, columns, W1, w1size, Mask, Tmp);
	convolve2D_hrow_with_mask(Tmp, rows, columns, W2, w2size, Mask, Out);


	mem_free(Tmp);
}
